#pragma once
#ifndef COLLLISION_DETECTOR_2D_H
#define COLLLISION_DETECTOR_2D_H

#include "basic_typedef.h"
#include <CGAL/squared_distance_2.h>
#include <CGAL/minkowski_sum_2.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <CGAL/Small_side_angle_bisector_decomposition_2.h>
#include "Clearance_detector_ie.h"
#include <CGAL/Arr_trapezoid_ric_point_location.h>

typedef CGAL::Aff_transformation_2<Kernel>  Transformation; 

#define USE_SMART_LOCAL_PLANNER
#define USE_TRAPEZODIAL_DECOMPOSITION

typedef Polygon_set_2::Arrangement_2 MyArrangement;
typedef CGAL::Arr_trapezoid_ric_point_location<MyArrangement> MyPointLocation;

class CollisionDetector2D {
public:
	
	CollisionDetector2D(Polygon_2 robot_polygon, Obstacles* obs, double epsilon = 1.0)
		: m_clearanceDetector(obs, robot_polygon, epsilon), m_epsilon(epsilon) {
		
		// Compute the Minkowski sum using the decomposition approach.
		CGAL::Small_side_angle_bisector_decomposition_2<Kernel>  ssab_decomp;

 		Polygon_2 approx_robot = robot_polygon;
		for (Obstacles::iterator iter = obs->begin(); iter != obs->end(); iter++)
		{
			Transformation t(CGAL::SCALING, -1);
			Polygon_2 negativeRobot = transform(t, approx_robot);
			// For every obstacle calculate its Minkowski sum with the "- robot"
			Polygon_with_holes_2	poly_wh = minkowski_sum_2(*iter, negativeRobot, ssab_decomp);

			// Add the result to the polygon set
			m_poly_set.join(poly_wh);
		}
#ifdef USE_TRAPEZODIAL_DECOMPOSITION
		generate_trapezodial_decomposition();
#endif
	}
	
	~CollisionDetector2D(){
#ifdef USE_TRAPEZODIAL_DECOMPOSITION
		delete m_trapezodial_decomposition;
#endif

	};

	////////////////////////
	// Queries
	////////////////////////
	
	/*	Check if the given configuration is collision free (returns true for free samples) */
	bool	valid_conf(Point_2 pos) const
	{
#ifdef USE_TRAPEZODIAL_DECOMPOSITION
		CGAL::Object obj = m_trapezodial_decomposition->locate(pos);
		//Arrangement_on_surface_2::Vertex_const_handle    v;
		//Arrangement_on_surface_2::Halfedge_const_handle  e;
		MyArrangement::Face_const_handle      f;
		  //std::cout << "The point " << pos << " is located ";
		  if (CGAL::assign (f, obj)) {
			  //std::cout << "in a face. Fictious? " << f->is_fictitious() << std::endl;
			  //std::cout << "in a face. Contained? " << f->contained() << std::endl;
			// q is located inside a face:
			if (f->is_unbounded()) {
			  //std::cout << "inside the unbounded face." << std::endl;
			  return true;
			} else {
			  //std::cout << "inside a bounded face." << std::endl;
			  return !f->contained();
			}
		  } else {
			  //Degenerate edge cases. Treat them as illegal to stay away from dangerous points.
			  return false;
		  }
			  
			//if (CGAL::assign (e, obj)) {
			//// q is located on an edge:
			//std::cout << "on an edge: " << e->curve() << std::endl;
		 // }
		 // else if (CGAL::assign (v, obj)) {
			//// q is located on a vertex:
			//if (v->is_isolated())
			//  std::cout << "on an isolated vertex: " << v->point() << std::endl;
			//else
			//  std::cout << "on a vertex: " << v->point() << std::endl;
		 // }
		 // else {
			//CGAL_assertion_msg (false, "Invalid object.");
		 // }
#else
		bool robot1Valid = m_poly_set.oriented_side(pos) == CGAL::ON_NEGATIVE_SIDE;
		//std::cout << "is valid : " << robot1Valid << std::endl;
		return robot1Valid;
#endif
	}

	Polygon_2 inflateLine(Point_2 src, Point_2 dst) const {
		double srcX = CGAL::to_double(src.x());
		double srcY = CGAL::to_double(src.y());
		double dstX = CGAL::to_double(dst.x());
		double dstY = CGAL::to_double(dst.y());

		double deltaX = dstX - srcX;
		double deltaY = dstY - srcY;
		double deltaScale = sqrt(deltaX * deltaX + deltaY * deltaY);
	
		double offsetX = deltaY / deltaScale * 0.001;
		double offsetY = deltaX / deltaScale * -0.001;
		Point_2 polyPoint[] = { src, Point_2(srcX + offsetX, srcY + offsetY), Point_2(dstX + offsetX, dstY + offsetY), dst };
		Polygon_2 inflatedPoly = Polygon_2(polyPoint, polyPoint+4);
		return inflatedPoly;
	}

	/* Does a line intersect a polygon set? */
	bool	intersect_line_with_polygon(Point_2 start, Point_2 target, const Polygon_set_2& polySet) const {
		if (start == target) {
			//std::cout << "Same points!" << std::endl;
			return false;
		}
		
		Polygon_2 line = inflateLine(start, target);
		Polygon_set_2& nonConstPolySet = const_cast<Polygon_set_2&>(polySet);
		bool retVal = nonConstPolySet.do_intersect(line);
		return retVal;
	}

	/*	Validate the connection between two configurations by 
		sampling along a line.
		Input:	* Start and target configurations
				* Eps value that determines the size of the steps 
					that are taken along the line
	*/
	bool	local_planner(Point_2 start, Point_2 target, double eps) const {
#ifdef USE_SMART_LOCAL_PLANNER
		bool robot1Collision = intersect_line_with_polygon(start, target, m_poly_set);
		return (!robot1Collision);
#else
		double x1 = CGAL::to_double(start.x());
		double y1 = CGAL::to_double(start.y());
		double x2 = CGAL::to_double(target.x());
		double y2 = CGAL::to_double(target.y());

		double distance = sqrt(pow(x1 - x2, 2) + pow(y1 - y2,2));

		double step_size = eps;

		// calculate how many steps are required
		int step_num = floor((distance - step_size) / step_size);
		double vx = x2 - x1;
		double vy = y2 - y1;

		for (int i = 1; i <= step_num; i++)
		{
			// generate a configuration for every step
			double offset =  (i * step_size) / (distance - step_size);
			double currx = x1 + vx * offset;
			double curry = y1 + vy * offset;

			Point_2 currentPos(currx, curry);

			// If an intermidiate configuration is invalid, return false
			if (!valid_conf(currentPos))
				return false;
		}

		// GREAT SUCCESS!
		return true;
#endif
	}

	double clearance_along_path(Point_2 start, Point_2 target, double eps = 1) const {
		double minClearance = 1000000;

		double x1 = CGAL::to_double(start.x());
		double y1 = CGAL::to_double(start.y());
		double x2 = CGAL::to_double(target.x());
		double y2 = CGAL::to_double(target.y());

		double distance = sqrt(pow(x1 - x2, 2) + pow(y1 - y2,2));

		double step_size = eps;

		// calculate how many steps are required
		int step_num = floor((distance - step_size) / step_size);
		double vx = x2 - x1;
		double vy = y2 - y1;

		for (int i = 1; i <= step_num; i++)
		{
			// generate a configuration for every step
			double offset =  (i * step_size) / (distance - step_size);
			double currx = x1 + vx * offset;
			double curry = y1 + vy * offset;

			// Check the clearance at the current spot
			Point_2 currentPos(currx, curry);
			Clearance_detector_ie& clearDet = const_cast<Clearance_detector_ie&>(m_clearanceDetector);
			Point_2_ie currentPosIe = clearDet.to_ie(currentPos);
			double currentClearance = clearDet.clearance(currentPosIe);

			// Update the minimal clearance if needed
			if (currentClearance < minClearance) {
				minClearance = currentClearance;
			}
		}

		return minClearance;
	}

	////////////////////////
	// Preprocessing
	////////////////////////
#ifdef USE_TRAPEZODIAL_DECOMPOSITION
	void generate_trapezodial_decomposition() {
		m_lookup_arr = m_poly_set.arrangement();
		m_trapezodial_decomposition = new MyPointLocation(m_lookup_arr);
	}
#endif

	////////////////////////
	// Data Members
	////////////////////////
	Polygon_set_2	m_poly_set; // Polygon set, for collision detection
	Clearance_detector_ie m_clearanceDetector;
	double m_epsilon;
#ifdef USE_TRAPEZODIAL_DECOMPOSITION
	MyArrangement						m_lookup_arr;
	MyPointLocation *					m_trapezodial_decomposition;
#endif
};

#endif
